#!/usr/bin/python
import serial
import logging
import time

class TReX:
    logger = logging.getLogger("rollo.pololu.TReX")
    l_fwd = 1
    l_rvs = 2
    r_fwd = 2
    r_rvs = 1
    speed_normal = 127
    baudrate=19200	
    sz_cmd_prefix_short = chr(0x86)
    sz_cmd_prefix_long = chr(0x80)+chr(0x07)
	
    b_cmd_accelerate = 0xE0
    b_cmd_read_channels =0x86
    b_data_all_channels=0x1F
    b_cmd_sig = 0x81
    b_cmd_currents = 0x8F

    lastAction = ''
	
    def __init__(self,pname='/dev/ttyS2',long_protocol=False,pololu_id=0x07):
        self.port = serial.Serial(pname)
        self.port.baudrate= self.baudrate
	self.port.stopbits = serial.STOPBITS_ONE
	self.port.timeout= 1
	self.logger.info("Connection established")
	self.set_protocol(long_protocol,pololu_id)

    def set_protocol(self, is_long=False, b_id=0x07):
	self.msb_clear=is_long
	if is_long:
   		self.sz_cmd_prefix= self.sz_cmd_prefix_long+chr(b_id)
	else:
   		self.sz_cmd_prefix= self.sz_cmd_prefix_short
		

    def cmd(self, cmd, data='',retlen=0):
        self.port.write(self.sz_cmd_prefix+chr(self.clear_msb_if_nec(cmd))+data)
	if(retlen>0):
		return self.read(retlen)

    def clear_msb_if_nec(self,cmd):
	if self.msb_clear:
		cmd &= ~(1 << 8)
		return cmd
	else:
		return cmd

    def read(self,count):
	return self.port.read(count)

    def accelerate_to(self, left_dir, right_dir, left_power, right_power):
	self.cmd(self.b_cmd_accelerate | left_dir*4 | right_dir, chr(left_power) + chr(right_power))


    def forwards(self):
        self.accelerate_to(self.l_fwd, self.r_fwd, self.speed_normal, self.speed_normal)
			
    def backwards(self):
        self.accelerate_to(self.l_rvs, self.r_rvs, self.speed_normal, self.speed_normal)
			
    def left(self):
        self.accelerate_to(self.l_fwd, self.r_rvs, self.speed_normal, self.speed_normal)
			
    def right(self):
        self.accelerate_to(self.l_rvs, self.r_fwd, self.speed_normal, self.speed_normal)

    def stop(self):
        self.accelerate_to(self.l_fwd, self.r_fwd, 0, 0)
   
			
    def get_sig(self):
        self.cmd(self.b_cmd_sig)
	return self.read(20)

    def get_raw_channels(self):
        self.cmd(self.b_cmd_read_channels, str(self.b_data_all_channels))
	return self.read(20)

    def get_currents(self):
        self.cmd(self.b_cmd_currents)
	return self.read(2)


class TReXTextController:
    logger = logging.getLogger("rollo.pololu.TReX")
    
    def __init__(self,trex):
	self.trex= trex

    def basic_action(self, thisAction):
	if self.lastAction != thisAction:
		if thisAction == "f":
			self.logger.debug("/\ Forwards")
			self.trex.forwards()
		elif thisAction == "b":
			self.logger.debug("\/ Backwards")
			self.trex.backwards()
		elif thisAction == "l":
			self.logger.debug("<< Left")
			self.trex.left()
		elif thisAction == "r":
			self.logger.debug(">> Right")
			self.trex.right()
		else:
			self.logger.debug(">< Stopped")
			self.trex.stop()
		self.lastAction = thisAction


